/*
@file: filters.cpp
@author: ZZH
@date: 2022-05-05
@info: 数字滤波器实现
*/
#include "libFun.h"

/*
@brief 一阶低通滤波器(采用一阶滞后滤波实现)
@param 采样序列, 截止频率
@return
*/
EXPORT void lpf(pFunCallArg_t pArgs, float* output)
{
    int allCalNum = pArgs->allCalNum;
    float* arg0 = static_cast<float*>(pArgs->args[0]);//采样序列
    float factor = *static_cast<float*>(pArgs->args[1]) / pArgs->fs;//滤波系数, 截止频率 / 采样频率
    float last = 0;

    for (int i = 0;i < allCalNum;i++)
    {
        output[i] = factor * arg0[i] + (1 - factor) * last;
        last = output[i];
    }
}

/*
@brief 一阶高通滤波器
@param
@return
*/
EXPORT void hpf(pFunCallArg_t pArgs, float* output)
{

}

/*
@brief 均值滤波器, 将整个采样序列划分为多个"均值单元", 从而形成新的采样序列, 新的序列里每个单元内部的平均值作为此单元的值加入序列
@param 采样序列, 均值取样点数
@return
*/
EXPORT void average(pFunCallArg_t pArgs, float* output)
{
    int allCalNum = pArgs->allCalNum;
    float* arg0 = static_cast<float*>(pArgs->args[0]);//采样序列
    int sampleNum = abs(*static_cast<float*>(pArgs->args[1]));//每个均值单元包括的点数
    int tail = allCalNum % sampleNum;//不满足数量要求的均值单元
    int sampleTimes = allCalNum / sampleNum;//均值单元的数量
    float cellRes = 0;
    int startIndex = 0;
    int endIndex = 0;

    if (sampleNum > allCalNum || sampleNum == 0)//参数错误
        return;

    for (int i = 0;i < sampleTimes;i++)
    {
        startIndex = i * sampleNum;
        endIndex = startIndex + sampleNum;

        for (int j = startIndex;j < endIndex;j++)//计算第N个均值单元的总和
            cellRes += arg0[j];

        cellRes /= sampleNum;//计算均值

        for (int j = startIndex;j < endIndex;j++)//以均值作为整个单元的值
            output[j] = cellRes;
    }

    if (0 != tail)//总点数不是采样点数的整数倍, 上面的处理会留下一块不够一个采样单元大小的数据, 这里处理掉
    {
        startIndex = endIndex;
        endIndex = startIndex + tail;

        for (int i = startIndex;i < endIndex;i++)
            cellRes += arg0[i];

        cellRes /= tail;

        for (int i = startIndex;i < endIndex;i++)
            output[i] = cellRes;
    }
}

/*
@brief FIR滤波器, 可使用MATLAB导出的滤波器参数
@param 采样序列, 滤波器参数(可使用read_file将参数读入)
@return
*/
EXPORT void fir(pFunCallArg_t pArgs, float* output)
{
    int allCalNum = pArgs->allCalNum;
    float* arg0 = static_cast<float*>(pArgs->args[0]);//采样序列
    int level = (int)*static_cast<float*>(pArgs->args[1]) + 1;
    float* factor = static_cast<float*>(pArgs->args[2]);
    float* x = new float[allCalNum + level];

    memset(x, 0, sizeof(float) * (allCalNum + level));
    x += level;
    memcpy(x, arg0, sizeof(float) * allCalNum);

    for (int i = 0;i < allCalNum;i++)//N
    {
        float sum = 0;
        for (int j = 0;j < level;j++)
            sum += x[i - j] * factor[j];
        output[i] = sum;
    }

    x -= level;
    delete[] x;
}

LibFunction_t funcs[] = {
    LIB_FUNCTION(lpf, 2),
    LIB_FUNCTION(hpf, 2),
    LIB_FUNCTION(fir, 3),
    LIB_FUNCTION(average, 2),
    END_OF_LIB
};

register_function_lib(funcs);


